#include"command.h"
#include "qdebug.h"

#include <QVector>
double g_xScale = 1.0;
double g_yScale = 1.0;
 
JOINT jointList[] = {
    JOINT{0 ,"髋关节侧摆","leg1_1","J_hip_l_roll",LEFT_LEG_JOINT},
    JOINT{1 ,"髋关节周转","leg1_2","J_hip_l_yaw",LEFT_LEG_JOINT},
    JOINT{2 ,"髋关节俯仰","leg1_3","J_hip_l_pitch",LEFT_LEG_JOINT},
    JOINT{3 ,"膝关节俯仰","leg1_4","J_knee_l_pitch",LEFT_LEG_JOINT},
    JOINT{4 ,"踝关节俯仰","leg1_5","J_ankle_l_pitch",LEFT_LEG_JOINT},
    JOINT{5 ,"踝关节侧摆","leg1_6","J_ankle_l_roll",LEFT_LEG_JOINT},

    JOINT{6 ,"髋关节侧摆","leg2_1","J_hip_r_roll",RIGHT_LEG_JOINT},
    JOINT{7 ,"髋关节周转","leg2_2","J_hip_r_yaw",RIGHT_LEG_JOINT},
    JOINT{8 ,"髋关节俯仰","leg2_3","J_hip_r_pitch",RIGHT_LEG_JOINT},
    JOINT{9 ,"膝关节俯仰","leg2_4","J_knee_r_pitch",RIGHT_LEG_JOINT},
    JOINT{10,"踝关节俯仰","leg2_5","J_ankle_r_pitch",RIGHT_LEG_JOINT},
    JOINT{11,"踝关节侧摆","leg2_6","J_ankle_r_roll",RIGHT_LEG_JOINT},

    JOINT{12,"肩关节俯仰","arm1_1","J_arm_l_01",LEFT_ARM_JOINT},
    JOINT{13,"肩关节侧摆","arm1_2","J_arm_l_02",LEFT_ARM_JOINT},
    JOINT{14,"肩关节周转","arm1_3","J_arm_l_03",LEFT_ARM_JOINT},
    JOINT{15,"肘关节展折","arm1_4","J_arm_l_04",LEFT_ARM_JOINT},
    JOINT{16,"肘关节周转","arm1_5","J_arm_l_05",LEFT_ARM_JOINT},
    JOINT{17,"腕关节俯仰","arm1_6","J_arm_l_06",LEFT_ARM_JOINT},
    JOINT{18,"腕关节周转","arm1_7","J_arm_l_07",LEFT_ARM_JOINT},

    JOINT{19,"肩关节俯仰","arm2_1","J_arm_r_01",RIGHT_ARM_JOINT},
    JOINT{20,"肩关节侧摆","arm2_2","J_arm_r_02",RIGHT_ARM_JOINT},
    JOINT{21,"肩关节周转","arm2_3","J_arm_r_03",RIGHT_ARM_JOINT},
    JOINT{22,"肘关节展折","arm2_4","J_arm_r_04",RIGHT_ARM_JOINT},
    JOINT{23,"肘关节周转","arm2_5","J_arm_r_05",RIGHT_ARM_JOINT},
    JOINT{24,"腕关节俯仰","arm2_6","J_arm_r_06",RIGHT_ARM_JOINT},
    JOINT{25,"腕关节周转","arm2_7","J_arm_r_07",RIGHT_ARM_JOINT},

    //毛辰飞更改20251113更改了顺序

    JOINT{26,"俯仰","waist_1","J_waist_pitch",WAIST_JOINT},
    JOINT{27,"侧摆","waist_2","J_waist_roll",WAIST_JOINT},
    JOINT{28,"周转","waist_3","J_waist_yaw",WAIST_JOINT},

    JOINT{29,"周转","head_1","J_head_yaw",HEAD_JOINT},
    JOINT{30,"俯仰","head_2","J_head_pitch",HEAD_JOINT},

    JOINT{0,"X","tip1_1","",LEFT_TIP_JOINT},
    JOINT{1,"Y","tip1_2","",LEFT_TIP_JOINT},
    JOINT{2,"Z","tip1_3","",LEFT_TIP_JOINT},
    JOINT{3,"RZ","tip1_4","",LEFT_TIP_JOINT},
    JOINT{4,"RY","tip1_5","",LEFT_TIP_JOINT},
    JOINT{5,"RX","tip1_6","",LEFT_TIP_JOINT},
    JOINT{0,"RZ","tip2_1","",RIGHT_TIP_JOINT},
    JOINT{1,"RY","tip2_2","",RIGHT_TIP_JOINT},
    JOINT{2,"RX","tip2_3","",RIGHT_TIP_JOINT},
    JOINT{3,"X","tip2_4","",RIGHT_TIP_JOINT},
    JOINT{4,"Y","tip2_5","",RIGHT_TIP_JOINT},
    JOINT{5,"Z","tip2_6","",RIGHT_TIP_JOINT},

    JOINT{0,"关节1","hand1_1","",LEFT_TOOL_HAND_JOINT},
    JOINT{1,"关节2","hand1_2","",LEFT_TOOL_HAND_JOINT},
    JOINT{2,"关节3","hand1_3","",LEFT_TOOL_HAND_JOINT},
    JOINT{3,"关节4","hand1_4","",LEFT_TOOL_HAND_JOINT},
    JOINT{4,"关节5","hand1_5","",LEFT_TOOL_HAND_JOINT},
    JOINT{5,"关节6","hand1_6","",LEFT_TOOL_HAND_JOINT},

    JOINT{0,"关节1","hand2_1","",RIGHT_TOOL_HAND_JOINT},
    JOINT{1,"关节2","hand2_2","",RIGHT_TOOL_HAND_JOINT},
    JOINT{2,"关节3","hand2_3","",RIGHT_TOOL_HAND_JOINT},
    JOINT{3,"关节4","hand2_4","",RIGHT_TOOL_HAND_JOINT},
    JOINT{4,"关节5","hand2_5","",RIGHT_TOOL_HAND_JOINT},
    JOINT{5,"关节6","hand2_6","",RIGHT_TOOL_HAND_JOINT},

    JOINT{0,"夹爪","hand1_1","",LEFT_TOOL_GRIPPER_JOINT},
    JOINT{0,"夹爪","hand2_1","",RIGHT_TOOL_GRIPPER_JOINT},

    JOINT{0,"吸盘","hand1_1","",LEFT_TOOL_SUCKER_JOINT},
    JOINT{0,"吸盘","hand2_1","",RIGHT_TOOL_SUCKER_JOINT},
    };

QVector<JOINT> getJointByType(JointType type)
{
    QVector<JOINT> list;
    for(int i = 0; i < 59; ++i)
    {
        if(jointList[i].type == type)
        {
            list.push_back(jointList[i]);
        }
    }
    return list;
}


QVector<JOINT> getJointBody()
{
    QVector<JOINT> list;
    for(int i = 0; i <= 30; ++i)
    {
        list.push_back(jointList[i]);
    }
    return list;
}

QString getJointTypeText(JointType type)
{
    switch (type) {
    case ROBOT_STATUS:
        return "状态";
    case HEAD_JOINT:
        return "头";
    case WAIST_JOINT:
        return "腰";
    case LEFT_LEG_JOINT:
        return "左腿";
    case RIGHT_LEG_JOINT:
        return "右腿";
    case LEFT_ARM_JOINT:
        return "左臂";
    case RIGHT_ARM_JOINT:
        return "右臂";
    case LEFT_TIP_JOINT:
        return "左末端";
    case RIGHT_TIP_JOINT:
        return "右末端";
    case LEFT_TOOL_JOINT:
    case LEFT_TOOL_HAND_JOINT:
        return "左手";
    case LEFT_TOOL_GRIPPER_JOINT:
        return "左夹爪";
    case LEFT_TOOL_SUCKER_JOINT:
        return "左吸盘";
    case RIGHT_TOOL_JOINT:
    case RIGHT_TOOL_HAND_JOINT:
        return "右手";
    case RIGHT_TOOL_GRIPPER_JOINT:
        return "右夹爪";
    case RIGHT_TOOL_SUCKER_JOINT:
        return "右吸盘";
    default:
        return "";
    }
}
